
#include <common/common.h>
#include <controller_manager.h>
#include <chassis_controller/chassis_controller.h>
#include <diff_drive_controller/diff_drive_controller.h>

namespace resource_manager {

ControllerManager::ControllerManager(rclcpp::Node &nh, ros_canopen_driver::HardwareInterface* hi)
{
    nh_ = nh;
    controllers_.resize(0);
    state_ = ControllerState::CONSTRUCTED;
    hi_ = hi;
    jhc_ = hi_->getJointHandleCollector();
}

ControllerManager::~ControllerManager()
{
	for (auto &controller : controllers_) {
		delete controller;
		controller = nullptr;
	}
}

bool ControllerManager::init()
{
	controllers_.resize(0);
	controller_init_futures_.clear();

	std::string controller_param_prefix = "/base/controllers";
	std::vector<std::string> controller_name_list;
	controller_name_list = common::get_single_layer_param_names(controller_param_prefix);
	node_status_sub_ = nh_.subscribe("/canopen_node_status", 10, &ControllerManager::node_status_callback, this);
	controller_enable_srv_ = nh_.advertiseService("/controller_enable", &ControllerManager::controller_enable_callback, this);

	if (controller_name_list.size() == 0) {
		RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[ControllerManager] cannot get param controller_list");
		return false;
	}

	if (!init_joint_states_publisher()) {
		RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[ControllerManager] init joint states publisher failed !");
		return false;
	}


	for (auto name : controller_name_list) {
		RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] get controller type: %s", name.c_str());

		rclcpp::Node c_nh;
		try{
			c_nh = rclcpp::Node(nh_, "controllers/" + name);
		}
		catch(std::exception &e) {
			RCLCPP_ERROR(rclcpp::get_logger("Driver"), "Exception thrown while constructing nodehandle for controller with name '%s':\n%s", name.c_str(), e.what());
			return false;
		}
		catch(...){
			RCLCPP_ERROR(rclcpp::get_logger("Driver"), "Exception thrown while constructing nodehandle for controller with name '%s'", name.c_str());
			return false;
		}

		ControllerBase* p = nullptr;
		
		if (name == "twist_controller") {
			std::string controller_param = nh_.getNamespace() + "/controllers/" + name + "/controller_name";
			std::string controller_name;
			nh_.param(controller_param, controller_name, std::string(""));
			if (controller_name.length() == 0) {
				RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[ControllerManager] cannot get param %s", controller_param.c_str());
				controllers_.resize(0);
				return false;
			}

			RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] get twist controller name: %s", controller_name.c_str());
			if (controller_name == "chassis_controller") {
				RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] construct controller: %s", controller_name.c_str());
				p = new chassis_controller::ChassisController(c_nh, jhc_);
				p->node_required.clear();
				p->controller_name = controller_name;

				int pivot_num = 0;
				c_nh.param("pivots_num", pivot_num, 0);

				if (pivot_num < 1) {
					RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[ControllerManager] chassis_controller get bad pivots_num: %d", pivot_num);
					return false;
				}

				for (int i=0; i<pivot_num; i++) {
					bool use_encoder = false;
					std::string use_encoder_param_name = "pivot_param" + std::to_string(i+1) + "/use_external_encoder";
					c_nh.param(use_encoder_param_name, use_encoder, false);
					
					if (use_encoder) {
						std::string encoder_joint_name = "";
						std::string encoder_joint_param_name = "pivot_param" + std::to_string(i+1) + "/external_encoder_name";
						c_nh.param(encoder_joint_param_name, encoder_joint_name, std::string(""));
						RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] get encoder joint name[%s] from param[%s]", encoder_joint_name.c_str(), 
							encoder_joint_param_name.c_str());
						
						int nodeid = -1;
						std::string nodeid_param_name = "/joint/" + encoder_joint_name + "/nodeId";
						nh_.param(nodeid_param_name, nodeid, -1);
						
						int manufacturer = -1;
						std::string manufacturer_param_name = "/joint/" + encoder_joint_name + "/manufacturer";
						nh_.param(manufacturer_param_name, manufacturer, -1);

						std::string busname = "";
						std::string busname_param_name = "/joint/" + encoder_joint_name + "/busname";
						nh_.param(busname_param_name, busname, std::string("can0"));
						
						if (nodeid < 0 || manufacturer < 0) {
							RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[ControllerManager] %s get bad nodeid[%d] or manufacturer[%d]", encoder_joint_name.c_str(),
								nodeid, manufacturer);
							return false;
						}
						else {
							NodeJointInfo nji;
							nji.busname = busname;
							nji.nodeid = nodeid;
							nji.manufacturer = manufacturer;
							nji.type = 0;
							nji.joint_name = encoder_joint_name;
							nji.servo_initialized = false;

							p->node_required.push_back(nji);		
						}
					}

					for (int j=0; j<2; j++) {
						std::string joint_name = "";
						std::string joint_param_name = "pivot_param" + std::to_string(i+1) + "/joint" + std::to_string(j+1) + "/name";
						c_nh.param(joint_param_name, joint_name, std::string(""));
						RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] get servo joint name[%s] from param[%s]", joint_name.c_str(), joint_param_name.c_str());

						int nodeid = -1;
						std::string nodeid_param_name = "/joint/" + joint_name + "/nodeId";
						nh_.param(nodeid_param_name, nodeid, -1);

						int manufacturer = -1;
						std::string manufacturer_param_name = "/joint/" + joint_name + "/manufacturer";
						nh_.param(manufacturer_param_name, manufacturer, -1);
						
						int type = -1;
						std::string type_param_name = "/joint/" + joint_name + "/type";
						nh_.param(type_param_name, type, -1);
						
						std::string busname = "";
						std::string busname_param_name = "/joint/" + joint_name + "/busname";
						nh_.param(busname_param_name, busname, std::string("can0"));

						RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] get node id[%d] from param[%s]", nodeid, nodeid_param_name.c_str());

						if (nodeid < 0 || manufacturer < 0 || type < 0) {
							RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[ControllerManager] %s get bad nodeid[%d] or manufacturer[%d] or type[%d]", joint_name.c_str(),
								nodeid, manufacturer, type);
							return false;
						}
						else {
							NodeJointInfo nji;
							nji.busname = busname;
							nji.nodeid = nodeid;
							nji.type = type;
							nji.manufacturer = manufacturer;
							nji.joint_name = joint_name;
							nji.servo_initialized = false;

							p->node_required.push_back(nji);		
						}
					}
				}
			}
			else if (controller_name == "diff_drive_controller") {
				RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] construct controller: %s", controller_name.c_str());
				p = new diff_drive_controller::DiffDriveController(c_nh, jhc_);
				p->node_required.clear();
				p->controller_name = controller_name;

				std::string left_joint_name;
				std::string right_joint_name;
				c_nh.param("left_wheel", left_joint_name, std::string(""));
				c_nh.param("right_wheel", right_joint_name, std::string(""));

				RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] get joint_name left[%s] right[%s]", left_joint_name.c_str(), right_joint_name.c_str());
								
				int left_nodeid = -1;
				int right_nodeid = -1;
				int left_manufacturer = -1;
				int right_manufacturer = -1;
				std::string left_busname = "";
				std::string right_busname = "";
				std::string left_nodeid_param_name = "/joint/" + left_joint_name + "/nodeId";
				std::string right_nodeid_param_name = "/joint/" + right_joint_name + "/nodeId";
				std::string left_manufacturer_param_name = "/joint/" + left_joint_name + "/manufacturer";
				std::string right_manufacturer_param_name = "/joint/" + right_joint_name + "/manufacturer";
				std::string left_busname_param_name = "/joint/" + left_joint_name + "/busname";
				std::string right_busname_param_name = "/joint/" + right_joint_name + "/busname";
				nh_.param(left_nodeid_param_name, left_nodeid, -1);
				nh_.param(right_nodeid_param_name, right_nodeid, -1);
				nh_.param(left_manufacturer_param_name, left_manufacturer, -1);
				nh_.param(right_manufacturer_param_name, right_manufacturer, -1);
				nh_.param(left_busname_param_name, left_busname, std::string("can0"));
				nh_.param(right_busname_param_name, right_busname, std::string("can0"));

				if (left_nodeid < 0 || right_nodeid < 0 || left_manufacturer < 0 || right_manufacturer < 0) {
					RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[ControllerManager] diff_drive_controller get bad node[%d][%d] manufacturer[%d][%d]",
						left_nodeid, right_nodeid, left_manufacturer, right_manufacturer);
					return false;
				}
				else {
					NodeJointInfo left_nji;
					left_nji.busname = left_busname;
					left_nji.nodeid = left_nodeid;
					left_nji.type = 0;
					left_nji.manufacturer = left_manufacturer;
					left_nji.joint_name = left_joint_name;
					left_nji.servo_initialized = false;

					NodeJointInfo right_nji;
					right_nji.busname = right_busname;
					right_nji.nodeid = right_nodeid;
					right_nji.type = 0;
					right_nji.manufacturer = right_manufacturer;
					right_nji.joint_name = right_joint_name;
					right_nji.servo_initialized = false;

					p->node_required.push_back(left_nji);
					p->node_required.push_back(right_nji);
				}
			}
		}
		else {
			std::string joint_name;
			std::string joint_param = nh_.getNamespace() + "/controllers/" + name + "/joint_name";
			nh_.param(joint_param, joint_name, std::string(""));
			if (joint_name.length() == 0) {
				RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[ControllerManager] cannot get param %s", joint_param.c_str());
				controllers_.resize(0);
				return false;
			}

			std::string controller_param = nh_.getNamespace() + "/controllers/" + name + "/controller_name";
			std::string controller_name;
			nh_.param(controller_param, controller_name, std::string(""));
			
			if (controller_name == "position_controller") {
				p = new PositionController(c_nh, jhc_, joint_name);
			}
			else if (controller_name == "velocity_controller") {
				p = new VelocityController(c_nh, jhc_, joint_name);
			}
			
			if (p != nullptr) {
				p->node_required.clear();
				p->controller_name = name;

				int nodeid = -1;
				std::string nodeid_param_name = "/joint/" + joint_name + "/nodeId";
				nh_.param(nodeid_param_name, nodeid, -1);
				
				int type = -1;
				std::string type_param_name = "/joint/" + joint_name + "/type";
				nh_.param(type_param_name, type, -1);
				
				int manufacturer = -1;
				std::string manufacturer_param_name = "/joint/" + joint_name + "/manufacturer";
				nh_.param(manufacturer_param_name, manufacturer, -1);

				std::string busname;
				std::string busname_param_name = "/joint/" + joint_name + "/busname";
				nh_.param(busname_param_name, busname, std::string("can0"));

				NodeJointInfo nji;
				nji.busname = busname;
				nji.nodeid = nodeid;
				nji.type = type;
				nji.manufacturer = manufacturer;
				nji.joint_name = joint_name;
				nji.servo_initialized = false;

				if (nodeid < 0 || manufacturer < 0 || type < 0) {
					RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[ControllerManager] %s get bad nodeid[%d] or manufacturer[%d] or type[%d]", joint_name.c_str(),
						nodeid, manufacturer, type);
					return false;
				}
				else {
					p->node_required.push_back(nji);
				}
			}
		}

		if (p != nullptr) {
			p->state = (int)ControllerState::CONSTRUCTED;
			controllers_.emplace_back(p);
		}
	}

	return true;
}

bool ControllerManager::init_joint_states_publisher()
{
	std::vector<std::string> joint_names = jhc_->getJointHandleNames();

	num_joints_ = joint_names.size();
	for (unsigned i=0; i<num_joints_; i++)
  	RCLCPP_DEBUG(rclcpp::get_logger("Driver"), "Got joint %s", joint_names[i].c_str());

	if (!nh_.getParam("joint_state_publish_rate", publish_rate_)){
	  RCLCPP_ERROR(rclcpp::get_logger("Driver"), "Parameter 'publish_rate' not set");
	  return false;
	}

	realtime_pub_.reset(new realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>(nh_, "joint_states", 4));

	realtime_pub_->msg_.name.resize(0);
	realtime_pub_->msg_.position.resize(0);
	realtime_pub_->msg_.velocity.resize(0);
	realtime_pub_->msg_.effort.resize(0);

	for (unsigned i=0; i<num_joints_; i++){
	  realtime_pub_->msg_.name.push_back(joint_names[i]);
	  realtime_pub_->msg_.position.push_back(0.0);
	  realtime_pub_->msg_.velocity.push_back(0.0);
	  realtime_pub_->msg_.effort.push_back(0.0);
	}

	return true;
}

void ControllerManager::publish_joint_states(const rclcpp::Time& time)
{
	if (publish_rate_ > 0.0 && last_publish_time_ + rclcpp::Duration(1.0/publish_rate_) < time){
		if (realtime_pub_->trylock()){
			last_publish_time_ = last_publish_time_ + rclcpp::Duration(1.0/publish_rate_);
			realtime_pub_->msg_.header.stamp = time;

			JointHandle* p;

			for (unsigned i=0; i<num_joints_; i++){
				p = jhc_->getHandle(realtime_pub_->msg_.name[i]);
				if (p) {
					realtime_pub_->msg_.position[i] = p->getPosition();
					realtime_pub_->msg_.velocity[i] = p->getVelocity();
					realtime_pub_->msg_.effort[i] = p->getEffort();
				}
				else {
					ROS_ERROR_THROTTLE(0.5, "[ControllerManager] cannot get joint handle!");
				}
			}
			realtime_pub_->unlockAndPublish();
		}
	}
}

void ControllerManager::update(const rclcpp::Time& time, const rclcpp::Duration& period)
{
	std::vector<std::string> unrunning_controllers;
	unrunning_controllers.clear();
	bool jacking_init_finished = true;

	for (auto controller : controllers_) {
		if (controller->controller_name.compare(0, 18, "jacking_controller") == 0) {
			jacking_init_finished &= (controller->state == (int)ControllerState::RUNNING);
		}
	}

	for (auto controller : controllers_) {
		std::string name = controller->controller_name;

		if (controller->state == (int)ControllerState::CONSTRUCTED) {
			if (controller_node_ready(controller)) {
				if (controller->servo_ready) {
					if (controller_init_futures_.find(name) == controller_init_futures_.end()) {
			            controller_init_futures_[name] = std::async(std::launch::async, std::bind(&ControllerBase::init, controller));
						controller->state = (int)ControllerState::INITIALIZING;
					}
				}
				else {
					bool is_rotary_controller = (controller->controller_name.length() >= 17) && (controller->controller_name.compare(0, 17, "rotary_controller") == 0);
					if (!is_rotary_controller || (is_rotary_controller && jacking_init_finished)) {
						init_servo(controller);
					}
					else {
						ROS_INFO_THROTTLE(1.0, "[ControllerManager] %s waiting for jacking_controller init.",
							controller->controller_name.c_str());
					}
				}
			}
			else {
				ROS_INFO_THROTTLE(1.0, "[ControllerManager] %s waiting for nodes ready ...", name.c_str());
				continue;
			}
		}
			
		if (controller->state == (int)ControllerState::INITIALIZING) {
			if (controller_init_futures_.find(name) != controller_init_futures_.end()) {
				std::future_status status = controller_init_futures_[name].wait_for(std::chrono::seconds(0));

				if (status == std::future_status::ready) {
					bool result = false;
					try {
						result = controller_init_futures_[name].get();
					}
					catch (const std::future_error& e) {
                        ROS_ERROR_STREAM("[ControllerManager] future for controller[" << name << "] get result error: " << e.what());
                        result = false;
                	}

                	if (result) {
                		RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] %s init succeed.", name.c_str());
                	}

                	controller->state = result ? (int)ControllerState::RUNNING : (int)ControllerState::CONSTRUCTED;
                	controller_init_futures_.erase(name);
				}
			}
			else {
				controller->state = (int)ControllerState::CONSTRUCTED;
			}
		}

		if (controller->state == (int)ControllerState::RUNNING || controller->state == (int)ControllerState::HALT) {
			controller->update(time, period);
		}
		else {
			unrunning_controllers.push_back(name);
			std::string names = std::string("");
			for (auto s : unrunning_controllers) {
				names += std::string(" ") + s;
			}			

			ROS_INFO_THROTTLE(1.0, "[ControllerManager] %s not running ...", names.c_str());
		}
	}

	publish_joint_states(time);
}


void ControllerManager::node_status_callback(const agv_msgs::msg::CANopenNodeStatus::ConstSharedPtr& msg)
{
	node_status_ = *msg;
}

bool ControllerManager::controller_node_ready(ControllerBase* controller)
{
	bool all_nodes_ready = true;
	std::vector<std::string> not_ready_joints;
	std::vector<std::string> all_joints;
	not_ready_joints.clear();
	all_joints.clear();
	for (const auto& nji : controller->node_required) {
		all_joints.push_back(nji.joint_name);
		bool node_ready = false;
		for (const auto& cns : node_status_.node_status) {
			if (nji.busname == cns.busname && nji.nodeid == cns.nodeid && cns.running_status == std::string("RUNNING")
				&& cns.communication_status == std::string("CONNECTED")) {
				node_ready = true;
			}
			else if (nji.busname == cns.busname && nji.nodeid == cns.nodeid) {
				not_ready_joints.push_back(nji.joint_name);
			}
		}

		all_nodes_ready &= node_ready;
	}

	std::string a_names = std::string("");
	for (auto& j : all_joints) {
		a_names += j + " ";
	}

	std::string n_names = std::string("");
	for (auto& s : not_ready_joints) {
		n_names += s + " ";
	}

	ROS_INFO_THROTTLE(1.0, "[ControllerManager] %s nodes %s  joints[%s]  not_ready[%s]", controller->controller_name.c_str(), 
		all_nodes_ready ? "connected" : "not connected", a_names.c_str(), n_names.c_str());
	return all_nodes_ready;
}


// unblocked function
bool ControllerManager::init_servo(ControllerBase* controller)
{
	bool finished = true;
	for (auto& nji : controller->node_required) {
		if (nji.manufacturer < 20) {
			if (!nji.servo_initialized && controller->init_futures.find(nji.joint_name) == controller->init_futures.end()) {
					RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] futures add servo[%s] type[%d]", nji.joint_name.c_str(), nji.type);
					controller->init_futures[nji.joint_name] = std::async(std::launch::async, std::bind(
						&ros_canopen_driver::HardwareInterface::servoInitSingle, hi_, std::placeholders::_1), nji.joint_name);		
			}

			if (!nji.servo_initialized && controller->init_futures.find(nji.joint_name) != controller->init_futures.end()) {
				std::future_status status = controller->init_futures[nji.joint_name].wait_for(std::chrono::seconds(0));

				if (status == std::future_status::ready) {
					bool result = false;
					try {
						result = controller->init_futures[nji.joint_name].get();
					}
					catch (const std::future_error& e) {
			            ROS_ERROR_STREAM("[ControllerManager] future for servo[" << nji.joint_name << "] get result error: " << e.what());
			            result = false;
			    	}

			    	if (result) {
			    		RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] servo[%s] init succeed.", nji.joint_name.c_str());
			    		nji.servo_initialized = true;
			    	}

			    	controller->init_futures.erase(nji.joint_name);
				}
			}

			finished &= nji.servo_initialized;
		}
	}

	if (finished)
		controller->servo_ready = true;

	return finished;
}

bool ControllerManager::halt_servo(ControllerBase* controller)
{	
	controller->halt_futures.clear();

	if (controller->state == (int)ControllerState::CONSTRUCTED || controller->state == (int)ControllerState::INITIALIZING) {
		RCLCPP_WARN(rclcpp::get_logger("Driver"), "[ControllerManager] halt: controller not initialized state[%d]", (int)controller->state);
		return true;
	}

	bool finished = true;
	for (auto& nji : controller->node_required) {
		if (nji.manufacturer < 20) {
			if (nji.servo_initialized) {
				RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] halt_futures add servo[%s]", nji.joint_name.c_str());
				controller->halt_futures[nji.joint_name] = std::async(std::launch::async, std::bind(
					&ros_canopen_driver::HardwareInterface::servoHaltSingle, hi_, std::placeholders::_1), nji.joint_name);		
			}
			else {
				controller->halt_futures.clear();
				RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[ControllerManager] controller[%s] joint[%s] not initialized.", controller->controller_name.c_str(),
					nji.joint_name.c_str());
				return false;
			}
		}
	}

	for (auto& nji : controller->node_required) {
		if (nji.manufacturer < 20) {
			bool result = false;
			try {
				result = controller->halt_futures[nji.joint_name].get();
			}
			catch (const std::future_error& e) {
	            ROS_ERROR_STREAM("[ControllerManager] halt_futures for servo[" << nji.joint_name << "] get result error: " << e.what());
	            result = false;
	    	}

	    	if (result) {
	    		RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] servo[%s] halt succeed.", nji.joint_name.c_str());
	    	}

			finished &= result;
		}
	}

	RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] controller[%s] switch to halt", controller->controller_name.c_str());
	controller->state = (int)ControllerState::HALT;
	return finished;
}

bool ControllerManager::recover_servo(ControllerBase* controller)
{
	controller->recover_futures.clear();
	if (controller->state == (int)ControllerState::CONSTRUCTED || controller->state == (int)ControllerState::INITIALIZING) {
		RCLCPP_WARN(rclcpp::get_logger("Driver"), "[ControllerManager] recover: controller not initialized state[%d]", (int)controller->state);
		return true;
	}
	

	bool finished = true;
	for (auto& nji : controller->node_required) {
		if (nji.manufacturer < 20) {
			if (nji.servo_initialized) {
				RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] recover_futures add servo[%s]", nji.joint_name.c_str());
				controller->recover_futures[nji.joint_name] = std::async(std::launch::async, std::bind(
					&ros_canopen_driver::HardwareInterface::servoRecoverSingle, hi_, std::placeholders::_1), nji.joint_name);		
			}
			else {
				controller->recover_futures.clear();
				RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[ControllerManager] controller[%s] joint[%s] not initialized.", controller->controller_name.c_str(),
					nji.joint_name.c_str());
				return false;
			}
		}
	}

	for (auto& nji : controller->node_required) {
		if (nji.manufacturer < 20) {
			bool result = false;
			try {
				result = controller->recover_futures[nji.joint_name].get();
			}
			catch (const std::future_error& e) {
	            ROS_ERROR_STREAM("[ControllerManager] recover_futures for servo[" << nji.joint_name << "] get result error: " << e.what());
	            result = false;
	    	}

	    	if (result) {
	    		RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] servo[%s] recover succeed.", nji.joint_name.c_str());
	    	}

			finished &= result;
		}
	}

	if (finished) {
		RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] controller[%s] switch to running", controller->controller_name.c_str());
		controller->state = (int)ControllerState::RUNNING;
	}

	return finished;
}


bool ControllerManager::controller_enable_callback(agv_srvs::srv::ControllerEnable::Request &req, agv_srvs::srv::ControllerEnable::Response &resp)
{
	bool result = true;

	ROS_INFO_STREAM("[ControllerManager] controller_enable_srv: " << req);
	for (auto c_name : req.controller_names) {
		bool existed = false;
		for (auto& c : controllers_) {
			RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ControllerManager] controller_name: %s  called_name: %s", c->controller_name.c_str(), c_name.c_str());
			if (c->controller_name == c_name) {
				existed = true;
				if (req.enable) {
					result &= recover_servo(c);
				}
				else {
					result &= halt_servo(c);
				}
			}
		}

		if (!existed)
			RCLCPP_WARN(rclcpp::get_logger("Driver"), "[ControllerManager] controller_enable_srv: cannot find controller[%s]", c_name.c_str());
	}

	resp.success = result;
	return resp.success;
}


}